#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import WrenchStamped
from rclpy.time import Time


class WrenchEcho(Node):
    def __init__(self):
        super().__init__('wrench_echo')
        self.sub = self.create_subscription(WrenchStamped, '/wrench_cmop', self.cb, 10)
        self.pub = self.create_publisher(WrenchStamped, '/wrench_cmop', 10)

    def cb(self, msg: WrenchStamped):
        # 打印一行 Fx, Fy, Fz
        print(f"Fx={msg.wrench.force.x:.3f}, Fy={msg.wrench.force.y:.3f}, Fz={msg.wrench.force.z:.3f}")
        # 改一下时间戳，避免被认为是同一条消息
        msg.header.stamp = self.get_clock().now().to_msg()
        self.pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = WrenchEcho()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
